{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "4f4bfde1",
   "metadata": {},
   "source": [
    "# High-Precision Propagation\n",
    "\n",
    "This example demonstrates high-precision orbit propagation using satkit's advanced numerical integration capabilities. It propagates a GPS satellite orbit over a 24-hour period and validates the results against high-fidelity SP3 ephemeris data from the European Space Agency (ESA).\n",
    "\n",
    "## Overview\n",
    "\n",
    "The example performs the following steps:\n",
    "\n",
    "1. **Loads Reference Data**: Reads GPS satellite positions from an SP3 file containing precise orbit determination results (note: velocity is not included)\n",
    "2. **Rotate to Inerital Frame**: Rotate the GPS satellite positions from the ITRF (Earth-fixed) to the GCRF (Inertial) frame\n",
    "3. **Fits Initial Conditions**: Uses numerical optimization to determine the initial velocity and solar radiation pressure coefficient (Cr·A/m) that best match the reference trajectory by integrating the initial state and minimizing the difference between the predicted and reported position\n",
    "4. **Validates Results**: Compares the propagated positions against the SP3 truth data and visualizes the position errors\n",
    "\n",
    "This example showcases satkit's ability to achieve meter-level accuracy over extended propagation periods when properly configured with high-fidelity force models and optimized initial conditions.\n",
    "\n",
    "The SP3 file contains a full 24 hours of satellite positions, recorded once every 5 minutes"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e97dd803",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Necessary imports\n",
    "import satkit as sk\n",
    "import numpy as np\n",
    "import math as m\n",
    "import numpy.typing as npt\n",
    "from scipy.optimize import minimize\n",
    "import plotly.graph_objects as go"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f318566b",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Function to read in the SP3 file\n",
    "\n",
    "def read_sp3file(fname, satnum=20):\n",
    "    \"\"\"\n",
    "    Read SP3 file\n",
    "    (file containing \"true\" GPS ephemerides)\n",
    "    and output UTC time and position in ITRF frame\n",
    "    \"\"\"\n",
    "\n",
    "    # Read in the test vectors\n",
    "    with open(fname, \"r\") as fd:\n",
    "        lines = fd.readlines()\n",
    "\n",
    "    def line2date(lines):\n",
    "        for line in lines:\n",
    "            year = int(line[3:7])\n",
    "            month = int(line[8:10])\n",
    "            day = int(line[11:13])\n",
    "            hour = int(line[14:16])\n",
    "            minute = int(line[17:19])\n",
    "            sec = float(line[20:32])\n",
    "            yield sk.time(year, month, day, hour, minute, sec)\n",
    "\n",
    "    def line2pos(lines):\n",
    "        for line in lines:\n",
    "            lvals = line.split()\n",
    "            yield np.array([float(lvals[1]), float(lvals[2]), float(lvals[3])])\n",
    "\n",
    "    datelines = list(filter(lambda x: x[0] == \"*\", lines))\n",
    "    match = f\"PG{satnum:02d}\"\n",
    "    satlines = list(filter(lambda x: x[0:4] == match, lines))\n",
    "    dates = np.fromiter(line2date(datelines), sk.time)\n",
    "    pitrf = np.stack(np.fromiter(line2pos(satlines), list), axis=0) * 1.0e3  # type: ignore\n",
    "\n",
    "    return (pitrf, dates)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "092c6ae3",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Download SP3 file if not present\n",
    "fname = './ESA0OPSFIN_20233640000_01D_05M_ORB.SP3'\n",
    "url = \"http://navigation-office.esa.int/products/gnss-products/2294/ESA0OPSFIN_20233640000_01D_05M_ORB.SP3.gz\"\n",
    "import os\n",
    "if not os.path.exists(fname):\n",
    "    import urllib.request\n",
    "    import gzip\n",
    "    with urllib.request.urlopen(url) as response:\n",
    "        with open(fname, 'wb') as out_file:\n",
    "            with gzip.GzipFile(fileobj=response) as uncompressed:\n",
    "                out_file.write(uncompressed.read())\n",
    "\n",
    "# Read in the SP3 file\n",
    "[pitrf, timearr] = read_sp3file(fname)\n",
    "\n",
    "\n",
    "# Rotate positions to the GCRF frame\n",
    "pgcrf = np.stack(\n",
    "    np.fromiter(\n",
    "        (q * p for q, p in zip(sk.frametransform.qitrf2gcrf(timearr), pitrf)), list # type: ignore\n",
    "    ),\n",
    "    axis=0,\n",
    ") # type: ignore\n",
    "# Crude estimation of initial velocity\n",
    "vgcrf = (pgcrf[1, :] - pgcrf[0, :]) / (timearr[1] - timearr[0]).seconds\n",
    "\n",
    "# Initial state for non-linear least squares is initial velocity\n",
    "# and susceptibility to radiation pressuer : Cr A / m\n",
    "# v0 = np.array([vgcrf[0], vgcrf[1], vgcrf[2], 0.02])\n",
    "\n",
    "# Skip ahead to the right answer... this takes a long time to run\n",
    "v0 = np.array([2.47130555e03, 2.94682777e03, -5.34171918e02, 2.13018578e-02])\n",
    "\n",
    "\n",
    "# Function to minimize ... takes the sum squared error between\n",
    "# propagated position and SP3 truth position\n",
    "def minfunc(v):\n",
    "    tstart = timearr[0]\n",
    "    tend = timearr[-1]\n",
    "    settings = sk.propsettings()\n",
    "    settings.gravity_order = 10 # type: ignore\n",
    "    settings.enable_interp = True # type: ignore\n",
    "    satprops = sk.satproperties_static(craoverm = v[3])\n",
    "\n",
    "    res = sk.propagate(\n",
    "        np.concatenate((pgcrf[0, :], v[0:3])),\n",
    "        begin=tstart,\n",
    "        end=tend,\n",
    "        propsettings=settings,\n",
    "        satproperties=satprops,\n",
    "    )\n",
    "    pest = np.zeros((len(timearr), 3))\n",
    "    for i in range(len(timearr)):\n",
    "        pest[i, :] = res.interp(timearr[i])[0:3]\n",
    "\n",
    "    return np.sum(np.sum((pest - pgcrf) ** 2, axis=1), axis=0)\n",
    "\n",
    "\n",
    "# Minimize difference between true satellite state and propagated state\n",
    "# by tuning initial velocity and CrAoverM\n",
    "r = minimize(minfunc, v0, method=\"Nelder-Mead\")\n",
    "\n",
    "# Propagate with optimized initial conditions\n",
    "settings = sk.propsettings()\n",
    "satprops = sk.satproperties_static(craoverm = r.x[3])\n",
    "res = sk.propagate(\n",
    "    np.concatenate((pgcrf[0, :], r.x[0:3])),\n",
    "    begin=timearr[0],\n",
    "    end=timearr[-1],\n",
    "    propsettings=settings,\n",
    "    satproperties=satprops,\n",
    ")\n",
    "perr = np.zeros((len(timearr), 3))\n",
    "for i in range(len(timearr)):\n",
    "    perr[i, :] = res.interp(timearr[i])[0:3] - pgcrf[i, :]\n",
    "# print the mean error\n",
    "print(\"Mean position error (m): \", np.mean(np.linalg.norm(perr, axis=1)))\n",
    "\n",
    "# Plot position error\n",
    "fig = go.Figure()\n",
    "\n",
    "fig.add_trace(\n",
    "    go.Scatter(x=[t.datetime() for t in timearr], y=perr[:, 0], mode=\"lines\", name=\"X\")\n",
    ")\n",
    "fig.add_trace(\n",
    "    go.Scatter(x=[t.datetime() for t in timearr], y=perr[:, 1], mode=\"lines\", name=\"Y\")\n",
    ")\n",
    "fig.add_trace(\n",
    "    go.Scatter(x=[t.datetime() for t in timearr], y=perr[:, 2], mode=\"lines\", name=\"Z\")\n",
    ")\n",
    "fig.update_layout(\n",
    "    title=\"Propagation Error vs SP3 Truth\",\n",
    "    xaxis_title=\"Time\",\n",
    "    yaxis_title=\"Position Error (m)\",\n",
    ")\n",
    "fig.show()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.14.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}